
#include <interrupt.h>
#include <rt_openamp.h>
#include <rthw.h>
#include <rtthread.h>

#define DBG_TAG "openamp"
#define DBG_LVL DBG_INFO
#include <rtdbg.h>

#define VDEV_STATUS_ADDR      AMP_VDEV_STATUS_ADDRESS

#define AMP_THREAD_STACK_SIZE 8192
#define AMP_THREAD_PRIORITY   5
#define AMP_THREAD_TICK       10

static int remote_cpu(void)
{
#ifdef RT_AMP_MASTER
    return AMP_SLAVE_CPU_ID;
#else
    return AMP_MASTRT_CPU_ID;
#endif
}

static int openamp_app_role_get(void)
{
#ifdef RT_AMP_MASTER
    return RPMSG_HOST;
#else
    return RPMSG_REMOTE;
#endif
}

static uint8_t Virtio_get_status(struct virtio_device *vdev)
{
    return HWREG8(VDEV_STATUS_ADDR);
}

static void Virtio_set_status(struct virtio_device *vdev, uint8_t status)
{
    HWREG8(VDEV_STATUS_ADDR) = status;
}

static uint32_t Virtio_get_features(struct virtio_device *vdev)
{
    return (1UL << (VIRTIO_RPMSG_F_NS));
}

static void Virtio_set_features(struct virtio_device *vdev, uint32_t features) {}

static void Virtio_notify(struct virtqueue *vq)
{
    struct rpmsg_virtio_device *rvdev;
    struct virtio_device       *vdev;
    struct rt_openamp          *app;

    vdev  = vq->vq_dev;
    rvdev = vdev->priv;
    app   = (struct rt_openamp *)rvdev;

    app->sigiface_ops->sigiface_send(app->remote_sgi_id, remote_cpu());
}

static const struct virtio_dispatch dispatch = {
    .get_status   = Virtio_get_status,
    .set_status   = Virtio_set_status,
    .get_features = Virtio_get_features,
    .set_features = Virtio_set_features,
    .notify       = Virtio_notify,
};

static void openamp_isr_handler(int vector, void *param)
{
    rt_sem_t openamp_sem = (rt_sem_t)param;
    LOG_D("openamp_sgi_isr_handler CPU#0");
    /* signal the semaphore */
    rt_sem_release(openamp_sem);
}

static void openamp_thread_entry(void *parameter)
{
    struct rt_openamp *app = (struct rt_openamp *)parameter;
    struct virtqueue  *rx_vq;

    if(rpmsg_virtio_get_role(&app->rvdev) == RPMSG_HOST)
    {
        rx_vq = app->rvdev.vdev->vrings_info[0].vq;
    }
    else
    {
        rx_vq = app->rvdev.vdev->vrings_info[1].vq;
    }

    while(1)
    {
        rt_sem_take(app->sem, RT_WAITING_FOREVER);

        /* openamp的virtual queue处理, 在任务上下午调用endpoint cb */
        virtqueue_notification(rx_vq);
    }
}

static void openamp_ns_bind_cb(struct rpmsg_device *rdev, const char *name, uint32_t dest)
{
    LOG_I("openamp_ns_bind_cb dest_addr = 0x%X name = %s", dest, name);
}

int openamp_app_create_endpoint(struct rpmsg_endpoint *ept, struct rpmsg_device *rdev, const char *name, uint32_t src, uint32_t dest, rpmsg_ept_cb cb,
                                rpmsg_ns_unbind_cb unbind_cb)
{
#ifdef RT_AMP_MASTER
    return rpmsg_create_ept(ept, rdev, name, src, dest, cb, unbind_cb);
#else
    return rpmsg_create_ept(ept, rdev, name, dest, src, cb, unbind_cb);
#endif
}

int openamp_app_dev_create(struct rt_openamp *app, char *name)
{
    struct metal_io_region       *shm_io;
    struct rpmsg_virtio_shm_pool *shpool;
    struct virtio_device         *vdev;
    struct virtio_vring_info     *vrings;
    struct virtqueue             *vq[2];
    rt_thread_t                   tid;

    shm_io = openamp_shmemio_get();

#ifdef AMP_AUTO_MEMORY
    app->vring_tx   = RT_ALIGN(AMP_SHARE_MEMORY_ADDRESS + app->vdev_status_size, app->align_size);
    app->vring_rx   = RT_ALIGN(app->vring_tx + (app->tx_num + 1) * 0x20, app->align_size);
    app->vring_buf  = RT_ALIGN(app->vring_rx + (app->rx_num + 1) * 0x20, app->align_size);
    app->vring_size = AMP_SHARE_MEMORY_ADDRESS + AMP_SHARE_MEMORY_SIZE - app->vring_buf;
#endif

    vq[0] = virtqueue_allocate(app->tx_num);
    if(!vq[0])
    {
        LOG_E("virtqueue_allocate failed to alloc vq[0]");
        return -1;
    }

    vq[1] = virtqueue_allocate(app->rx_num);
    if(!vq[1])
    {
        LOG_E("virtqueue_allocate failed to alloc vq[1]");
        return -1;
    }

    vrings = metal_allocate_memory(sizeof(struct virtio_vring_info) * 2);
    rt_memset(vrings, 0, sizeof(struct virtio_vring_info) * 2);
    vrings[0].io             = shm_io;
    vrings[0].info.vaddr     = (void *)(app->vring_tx);
    vrings[0].info.num_descs = app->tx_num;
    vrings[0].info.align     = 4;
    vrings[0].vq             = vq[0];

    vrings[1].io             = shm_io;
    vrings[1].info.vaddr     = (void *)(app->vring_rx);
    vrings[1].info.num_descs = app->rx_num;
    vrings[1].info.align     = 4;
    vrings[1].vq             = vq[1];

    vdev = metal_allocate_memory(sizeof(struct virtio_device));
    rt_memset(vdev, 0, sizeof(struct virtio_device));
    vdev->role        = openamp_app_role_get();
    vdev->vrings_num  = 2;
    vdev->func        = &dispatch;
    vdev->vrings_info = vrings;

    shpool = rt_malloc(sizeof(struct rpmsg_virtio_shm_pool));
    rpmsg_virtio_init_shm_pool(shpool, (void *)app->vring_buf, (size_t)app->vring_size);
    if(app->ns_bind_cb)
    {
        rpmsg_init_vdev(&app->rvdev, vdev, app->ns_bind_cb, shm_io, shpool);
    }
    else
    {
        rpmsg_init_vdev(&app->rvdev, vdev, openamp_ns_bind_cb, shm_io, shpool);
    }

    app->sem = rt_sem_create(name, 0, RT_IPC_FLAG_FIFO);
    if(app->sem == RT_NULL)
    {
        return -RT_ERROR;
    }

    app->sigiface_ops->sigiface_init(app->local_sgi_id, app->sem, openamp_isr_handler);

    tid = rt_thread_create(name, openamp_thread_entry, app, AMP_THREAD_STACK_SIZE, AMP_THREAD_PRIORITY, AMP_THREAD_TICK);
    if(tid == RT_NULL)
    {
        LOG_E("openamp thread create failed!");
        return -RT_ERROR;
    }

    rt_thread_startup(tid);

    return 0;
}

void openamp_app_ept_delete(struct rpmsg_endpoint *ept)
{
    rpmsg_destroy_ept(ept);
}
